﻿using System;
using System.Collections.Generic;
using Allegro.MathInterface;

namespace Allegro.MathLib
{
    public class Spline
    {
        private double[] _x = null;
        private double[] _y = null;
        private double _dy1;
        private double _dyn;
        private int _n = 0; // Array dimension
        private double[] _dy2 = null;
        private double[] _u = null;
        private int _klo = 0;
        private int _khi = 1;

       
        /// <summary>
        /// Construct a natural spline with zero second derivatives on the boundary
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        public Spline(double[] x, double[] y) : this(x, y, 1.0e30, 1.0e30)
        {
            
        }

        /// <summary>
        /// Given arrays x[1..n] and y[1..n] containing a tabulated function, i.e. y_i = f(x_i), with
        /// x1<x2<...<xN, and given values dy1 and dyn for the first derivative of the interpolating function at points
        /// 1 and n, respectively, this routine calculates an array y2[1..n] that contains the second derivatives of
        /// the interpolating functiona at the tabulated points x_i. 
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="dy1"></param>
        /// <param name="dyn"></param>
        public Spline(double[] x, double[] y, double dy1, double dyn)
        {
            _n = x.GetLength(0);
            if(_n < 2)
            {
                throw new ArgumentException(
                    "Unable to create a spline unless the arrays contain two or more elements");
            }
            if(y.GetLength(0) != _n)
            {
                throw new ArgumentException(
                    "Inconsistent arrays given as arguments to Spline constuctor. Arrays were of different length");
            }

            double p, qn, sig, un;
            _x = x;
            _y = y;
            _dy1 = dy1;
            _dyn = dyn;
            _u = new double[_n-1];
            _dy2 = new double[_n];

            if (dy1 > 0.99e30)
		        _dy2[0]=_u[0]=0.0;
	        else 
            {
		        _dy2[0] = -0.5;
		        _u[0]=(3.0/(x[1]-x[0]))*((y[1]-y[0])/(x[1]-x[0]) - dy1);
	        }
            for (int i=1; i < _n-1; i++) 
            {
		        sig = (x[i]-x[i-1])/(x[i+1]-x[i-1]);
		        p = sig*_dy2[i-1] + 2.0;
		        _dy2[i]=(sig - 1.0)/p;
		        _u[i] = (y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		        _u[i] = (6.0*_u[i]/(x[i+1]-x[i-1]) - sig*_u[i-1])/p;
	        }
	        if (dyn > 0.99e30)
		        qn=un=0.0;
	        else 
            {
		        qn=0.5;
		        un=(3.0/(x[_n-1]-x[_n-2]))*(dyn - (y[_n-1]-y[_n-2])/(x[_n-1]-x[_n-2]));
	        }
	        _dy2[_n-1]=(un - qn*_u[_n-2])/(qn*_dy2[_n-2] + 1.0);
	        for (int k = _n-2; k >= 0; k--)
	        {
	            _dy2[k] = _dy2[k]*_dy2[k + 1] + _u[k];
	        }
        }
        public Spline(Spline source) : this(source.x, source.y, source.u, source.dy2)
        {
            
        }
        internal Spline(double[] x, double[] y, double[] u, double[] dy2)
        {
            _n = x.GetLength(0);
            if (_n < 2)
            {
                throw new ArgumentException(
                    "Unable to create a spline unless the arrays contain two or more elements");
            }
            if (y.GetLength(0) != _n)
            {
                throw new ArgumentException(
                    "Inconsistent arrays given as arguments to Spline constuctor. Arrays were of different length");
            }

            _x = x;
            _y = y;
            _u = u;
            _dy2 = dy2;
        }
        /// <summary>
        /// Return the interpolation value at point x
        /// </summary>
        /// <param name="x"></param>
        /// <returns></returns>
        public double Value(double x)
        {
            int k;
            int klo = _klo;
            int khi = _khi;
            double h, b, a;
            // We will find the right place in the table by means of bisection. This is optimal if sequential calls
            // to this routine are at random values of x. If sequential calls are in order, and closely spaced, one
            // would do better to store previous values of klo and khi and test if they remain appropriate on the next
            // call.

            // Check previous table entry
            if(_x[klo] <= x && _x[khi] > x)
            {
                // x still falls in current bin, klo and khi still hold
            }
            else if (klo - 1 >= 0 && _x[klo - 1] <= x && _x[klo] > x)
            {
                // x falls in next lower bin
                khi = klo;
                klo -= 1;
            }
            else if (khi + 1 < _n && _x[khi] <= x && _x[khi + 1] > x)
            {
                // x falls in next higher bin
                klo = khi;
                khi += 1;
            }
            else
            {
                // No match, full binary search
                klo = 0;
                khi = _n - 1;
                while (khi - klo > 1)
                {
                    k = (khi + klo) >> 1;
                    if (_x[k] > x)
                        khi = k;
                    else
                        klo = k;
                }
            }
            h = _x[khi] - _x[klo];
            if(h == 0.0)
                throw new ArgumentException("x array contains two identical values. Unable to interpolate");

            a = (_x[khi] - x)/h;
            b = (x - _x[klo])/h; 
            // Cubic spline polynomial is now evaluated
            double y = a * _y[klo] + b * _y[khi] +
                       ((a * a * a - a) * _dy2[klo] + (b * b * b - b) * _dy2[khi]) * (h * h) / 6.0;
            _klo = klo;
            _khi = khi;
            return y;
        }

        /// <summary>
        /// Use spline to calculate the integral : 
        /// integral = 1/2 h (y0 + y1) - h^3/24 (y21 + y22)
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public double Integrate(double a, double b)
        {
            double intf = 0.0;
            int klo_a, khi_a, klo_b, khi_b;
            if (b > a)
            {
                double yb = Value(b);
                klo_b = _klo;
                khi_b = _khi;
                double ya = Value(a);
                klo_a = _klo;
                khi_a = _khi;
                if (klo_a == klo_b && khi_a == khi_b)
                {
                    // Pathological case: a and b fall in same bin
                    double h = (b - a);
                    double dy2 = (_dy2[klo_a] + _dy2[khi_a]) * h / (_x[khi_a] - _x[klo_a]);
                    intf = 0.5 * h * (ya + yb - (h * h) * dy2 / 24.0);
                }
                else
                {
                    double h = (_x[khi_a] - a);
                    double dy2 = (_dy2[klo_a] + _dy2[khi_a]) * h / (_x[khi_a] - _x[klo_a]);
                    intf = 0.5 * h * (ya + _y[khi_a] - (h * h) * dy2 / 24.0);

                    for (int i = khi_a; i < klo_b; i++)
                    {
                        h = (_x[i + 1] - _x[i]);
                        intf += 0.5 * h * (_y[i] + _y[i + 1] - (h * h) * (_dy2[i] + _dy2[i + 1]) / 24.0);
                    }

                    h = (b - _x[klo_b]);
                    dy2 = (_dy2[klo_b] + _dy2[khi_b]) * h / (_x[khi_b] - _x[klo_b]);
                    intf += 0.5 * h * (_y[klo_b] + yb - (h * h) * dy2 / 24.0);
                }
            }
            else
            {
                throw new NotImplementedException("Spline.Integrate: Backwards integration along a spline has not been implemented");
            }
            return intf;
        }

        /// <summary>
        /// Compute the integral Integral[f[x] * y[x],{x, a, b}]. Assume that f[x] is slower varying than y
        /// </summary>
        /// <param name="f"></param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public double Integrate(Func<double, double> f, double a, double b)
        {
            double intf = 0.0;
            int klo_a, khi_a, klo_b, khi_b;
            if (b > a)
            {
                double yb = Value(b);
                klo_b = _klo;
                khi_b = _khi;
                double ya = Value(a);
                klo_a = _klo;
                khi_a = _khi;
                if (klo_a == klo_b && khi_a == khi_b)
                {
                    // Pathological case: a and b fall in same bin
                    double h = (b - a);
                    double x = 0.5 * (a + b);
                    double fx = f(x);
                    double dy2 = (_dy2[klo_a] + _dy2[khi_a]) * h / (_x[khi_a] - _x[klo_a]);
                    intf = 0.5 * h * (ya + yb - (h * h) * dy2 / 24.0) * fx;
                }
                else
                {
                    double h = (_x[khi_a] - a);
                    double x = 0.5 * (a + _x[khi_a]);
                    double fx = f(x);
                    double dy2 = (_dy2[klo_a] + _dy2[khi_a]) * h / (_x[khi_a] - _x[klo_a]);
                    intf = 0.5 * h * (ya + _y[khi_a] - (h * h) * dy2 / 24.0) * fx;

                    for (int i = khi_a; i < klo_b; i++)
                    {
                        h = (_x[i + 1] - _x[i]);
                        x = 0.5 * (_x[i] + _x[i + 1]);
                        fx = f(x);
                        intf += 0.5 * h * (_y[i] + _y[i + 1] - (h * h) * (_dy2[i] + _dy2[i + 1]) / 24.0) * fx;
                    }

                    h = (b - _x[klo_b]);
                    x = 0.5 * (_x[klo_b] + b);
                    fx = f(x);
                    dy2 = (_dy2[klo_b] + _dy2[khi_b]) * h / (_x[khi_b] - _x[klo_b]);
                    intf += 0.5 * h * (_y[klo_b] + yb - (h * h) * dy2 / 24.0) * fx;
                }
            }
            else
            {
                throw new NotImplementedException("Spline.Integrate: Backwards integration along a spline has not been implemented");
            }
            return intf;
        }

        /// <summary>
        /// Get the source x values from the spline
        /// </summary>
        public double[] x
        {
            get{ return _x;}
        }
        /// <summary>
        /// Get the source y values from the spline
        /// </summary>
        public double[] y
        {
            get{ return _y;}
        }

        public double[] u
        {
            get{ return _u;}
        }
        public double[] dy2
        {
            get{ return _dy2;}
        }

        /// <summary>
        /// Current low index value
        /// </summary>
        public int iLow
        {
            get { return _klo; }
        }
        /// <summary>
        /// Current high index value
        /// </summary>
        public int iHigh
        {
            get{ return _khi;}
        }
        // Private methods
        private void _Locate(double x, out int klo, out int khi)
        {
            int k;
            klo = 0;
            khi = _n - 1;
            while (khi - klo > 1)
            {
                k = (khi + klo) >> 1;
                if (_x[k] > x)
                    khi = k;
                else
                    klo = k;
            }
        }
    }
}
